
/*
   DRV8833 for  MiniCar with Arduino Pro Mini
     * https://github.com/TheArduinist/DRV8833
*/
#ifndef DRV8833_H
#define DRV8833_H

// 3, 5, 6, 9, 10, or 11
#define LEFT_FORWARD 5  // BIN1
#define LEFT_BACK 3     // BIN2

#define RIGHT_FORWARD 6  //  AIN1
#define RIGHT_BACK 9     // AIN2

// car action code
const int CAR_FORWARD = 1;
const int CAR_BACKWARD = 2;
const int CAR_STEP_LEFT = 3;
const int CAR_STEP_RIGHT = 4;
const int CAR_ON_LEFT = 30;
const int CAR_ON_RIGHT = 40;
const int CAR_CIRCLE_LEFT = 31;
const int CAR_CIRCLE_RIGHT = 41;
const int CAR_STOP = 5;
// speed
const int CAR_SPEED_UP = 9;
const int CAR_SPEED_DOWN = 8;
const int CAR_LEFT_SPEED_UP = 381;
const int CAR_LEFT_SPEED_DOWN = 380;
const int CAR_RIGHT_SPEED_UP = 491;
const int CAR_RIGHT_SPEED_DOWN = 490;

// car running state
int car_state = CAR_STOP;
int cur_car_cmd = CAR_STOP;

// PWM - speed
int left_speed = 200;
int right_speed = 200;
const int speed_step = 10;

void car_forward() {
  if (left_speed > right_speed) {
    right_speed = left_speed;
  } else {
    left_speed = right_speed;
  }
  analogWrite(LEFT_FORWARD, left_speed);
  digitalWrite(LEFT_BACK, LOW);
  analogWrite(RIGHT_FORWARD, right_speed);
  digitalWrite(RIGHT_BACK, LOW);
}

void car_back() {
  if (left_speed > right_speed) {
    right_speed = left_speed;
  } else {
    left_speed = right_speed;
  }
  digitalWrite(LEFT_FORWARD, LOW);
  analogWrite(LEFT_BACK, left_speed);
  digitalWrite(RIGHT_FORWARD, LOW);
  analogWrite(RIGHT_BACK, right_speed);
}

void car_stop() {
  digitalWrite(LEFT_FORWARD, LOW);
  digitalWrite(LEFT_BACK, LOW);
  digitalWrite(RIGHT_FORWARD, LOW);
  digitalWrite(RIGHT_BACK, LOW);
}

// zero turn
void car_turn_left() {
  analogWrite(LEFT_FORWARD, 0);
  digitalWrite(LEFT_BACK, LOW);
  analogWrite(RIGHT_FORWARD, right_speed);
  digitalWrite(RIGHT_BACK, LOW);
}


void car_turn_step_left() {
  car_turn_left();
  car_state = CAR_STEP_LEFT;
  delay(100);
  car_stop();
}

// zero turn
void car_turn_right() {
  analogWrite(LEFT_FORWARD, left_speed);
  digitalWrite(LEFT_BACK, LOW);
  analogWrite(RIGHT_FORWARD, 0);
  digitalWrite(RIGHT_BACK, LOW);
}

void car_turn_step_right() {
  car_turn_right();
  car_state = CAR_STEP_RIGHT;
  delay(100);
  car_stop();
}

void car_circle_left() {
  digitalWrite(LEFT_FORWARD, LOW);
  analogWrite(LEFT_BACK, left_speed);
  analogWrite(RIGHT_FORWARD, right_speed);
  digitalWrite(RIGHT_BACK, LOW);
}

void car_circle_right() {
  analogWrite(LEFT_FORWARD, left_speed);
  digitalWrite(LEFT_BACK, LOW);
  digitalWrite(RIGHT_FORWARD, LOW);
  analogWrite(RIGHT_BACK, right_speed);
}

void running_with_new_speed() {
  switch (car_state) {
    case CAR_FORWARD:
      car_forward();
      break;
    case CAR_BACKWARD:
      car_back();
      break;
    case CAR_STEP_LEFT:
      car_turn_step_left();
      break;
    case CAR_STEP_RIGHT:
      car_turn_step_right();
      break;
    default:
      break;
  }
}

void car_left_speed_up() {
  left_speed += speed_step;
  if (left_speed > 255) {
    left_speed = 255;
  }
  running_with_new_speed();
}

void car_right_speed_up() {
  right_speed += speed_step;
  if (right_speed > 255) {
    right_speed = 255;
  }
  running_with_new_speed();
}

void car_speed_up() {
  right_speed += speed_step;
  if (right_speed > 255) {
    right_speed = 255;
  }

  left_speed += speed_step;
  if (left_speed > 255) {
    left_speed = 255;
  }
  running_with_new_speed();
}

void car_left_speed_down() {
  left_speed -= speed_step;
  if (left_speed < 0) {
    left_speed = 0;
  }
  running_with_new_speed();
}

void car_right_speed_down() {
  right_speed -= speed_step;
  if (right_speed < 0) {
    right_speed = 0;
  }
  running_with_new_speed();
}

void car_speed_down() {
  right_speed -= speed_step;
  if (right_speed < 0) {
    right_speed = 0;
  }

  left_speed -= speed_step;
  if (left_speed < 0) {
    left_speed = 0;
  }
  running_with_new_speed();
}

void car_action(int car_cmd) {
  switch (car_cmd) {
    case CAR_FORWARD:
      car_forward();
      car_state = CAR_FORWARD;
      break;
    case CAR_ON_LEFT:
      car_turn_left();
      car_state = CAR_ON_LEFT;
      break;
    case CAR_ON_RIGHT:
      car_turn_right();
      car_state = CAR_ON_RIGHT;
      break;
    case CAR_STEP_LEFT:
      car_turn_step_left();
      car_state = CAR_STOP;
      break;
    case CAR_STEP_RIGHT:
      car_turn_step_right();
      car_state = CAR_STOP;
      break;
    case CAR_BACKWARD:
      car_back();
      car_state = CAR_BACKWARD;
      break;
    case CAR_STOP:
      car_stop();
      car_state = CAR_STOP;
      break;
    case CAR_SPEED_UP:
      car_speed_up();
      break;
    case CAR_SPEED_DOWN:
      car_speed_down();
      break;
    case CAR_LEFT_SPEED_UP:
      car_left_speed_up();
      break;
    case CAR_LEFT_SPEED_DOWN:
      car_left_speed_down();
      break;
    case CAR_RIGHT_SPEED_UP:
      car_right_speed_up();
      break;
    case CAR_RIGHT_SPEED_DOWN:
      car_right_speed_down();
      break;
    default:
      break;
  }  // switch
};

void setup_drv8833() {
  pinMode(LEFT_FORWARD, OUTPUT);
  pinMode(LEFT_BACK, OUTPUT);
  pinMode(RIGHT_FORWARD, OUTPUT);
  pinMode(RIGHT_BACK, OUTPUT);
  car_stop();
}

void loop_drv8833() {
}

#endif /* DRV8833_H */